home *** CD-ROM | disk | FTP | other *** search
/ NetNews Offline 2 / NetNews Offline Volume 2.iso / news / comp / lang / c++-part1 / 9523 / Fourier.uue < prev    next >
Encoding:
Text File  |  1996-08-05  |  4.5 KB  |  204 lines
  1. #include <math.h>
  2. #include <values.h>
  3.  
  4. void InitFFT(int n);
  5. void FFT(double far *Input,double far *OutputRe,double far *OutputIm);
  6. void CleanUpFFT(void);
  7. void IFFT(double far *InputRe,double far *InputIm,double far *Output);
  8.  
  9.  
  10. static double *ReplaceXData;
  11. static double *ReplaceYData;
  12. static double *SinTwids;
  13. static int *UnscrambledIndex;
  14. static int N,N_DIV_4,LOG2Ndiv2;
  15.  
  16. void InitFFT(int n)
  17. {
  18.   int i,j;
  19.   N=n;
  20.   N_DIV_4=N/4;
  21.   LOG2Ndiv2=(int(log(N)/log(2))+1)>>1; //Juist afronden en snel
  22.  
  23.   ReplaceXData=new double[N];
  24.   ReplaceYData=new double[N];
  25.   SinTwids=new double[N+N/4];
  26.   UnscrambledIndex=new int[N];
  27.  
  28.   for (i=0;i<N+N_DIV_4;i++) SinTwids[i]=sin(i*(2*M_PI/N));
  29.   for (i=0;i<N;i++)
  30.   {
  31.     UnscrambledIndex[i]=0;
  32.     for (j=0;j<LOG2Ndiv2;j++)
  33.       UnscrambledIndex[i]|=((i>>((LOG2Ndiv2-1-j)<<1))&0x3)<<(j<<1);
  34.   }
  35.  
  36. }
  37.  
  38. void CleanUpFFT(void)
  39. {
  40.   delete [] ReplaceXData;
  41.   delete [] ReplaceYData;
  42.   delete [] SinTwids;
  43.   delete [] UnscrambledIndex;
  44. }
  45.  
  46. void FFT(double far *Input,double far *OutputRe,double far *OutputIm)
  47. {
  48.   int BFlys=N_DIV_4,BFlysBy3=(N*3)/4;
  49.   int Groups=1,GroupsBy2=2,GroupsBy3=3;
  50.   int Stage,Group,BFly;
  51.   int i;
  52.  
  53.   double *XA,*XB,*XC,*XD;
  54.   double *YA,*YB,*YC,*YD;
  55.   double xa,xb,xc,xd;
  56.   double ya,yb,yc,yd;
  57.   double *Cb,*Cc,*Cd;
  58.   double *Sb,*Sc,*Sd;
  59.  
  60.   for (i=0;i<N;i++) ReplaceXData[i]=Input[i];
  61.   for (i=0;i<N;i++) ReplaceYData[i]=0;
  62.  
  63.   for (Stage=LOG2Ndiv2;Stage;Stage--)
  64.   {
  65.     XA=ReplaceXData;
  66.     XB=XA+BFlys;
  67.     XC=XB+BFlys;
  68.     XD=XC+BFlys;
  69.     YA=ReplaceYData;
  70.     YB=YA+BFlys;
  71.     YC=YB+BFlys;
  72.     YD=YC+BFlys;
  73.     for(Group=Groups;Group;Group--)
  74.     {
  75.       Cb=Cc=Cd=SinTwids+N_DIV_4;
  76.       Sb=Sc=Sd=SinTwids;
  77.       for(BFly=BFlys;BFly;BFly--)
  78.       {
  79.         xa=*XA; xb=*XB; xc=*XC; xd=*XD;
  80.         ya=*YA; yb=*YB; yc=*YC; yd=*YD;
  81.  
  82.         *XA++=xa+xb+xc+xd;
  83.         *YA++=ya+yb+yc+yd;
  84.         *XB++=(xa+yb-xc-yd)*(*Cb)+(ya-xb-yc+xd)*(*Sb);
  85.         *YB++=(ya-xb-yc+xd)*(*Cb)-(xa+yb-xc-yd)*(*Sb);
  86.         *XC++=(xa-xb+xc-xd)*(*Cc)+(ya-yb+yc-yd)*(*Sc);
  87.         *YC++=(ya-yb+yc-yd)*(*Cc)-(xa-xb+xc-xd)*(*Sc);
  88.         *XD++=(xa-yb-xc+yd)*(*Cd)+(ya+xb-yc-xd)*(*Sd);
  89.         *YD++=(ya+xb-yc-xd)*(*Cd)-(xa-yb-xc+yd)*(*Sd);
  90.  
  91.         Cb+=Groups;
  92.         Cc+=GroupsBy2;
  93.         Cd+=GroupsBy3;
  94.  
  95.         Sb+=Groups;
  96.         Sc+=GroupsBy2;
  97.         Sd+=GroupsBy3;
  98.       }  /* ButterFly */
  99.  
  100.       XA+=BFlysBy3;
  101.       XB+=BFlysBy3;
  102.       XC+=BFlysBy3;
  103.       XD+=BFlysBy3;
  104.  
  105.       YA+=BFlysBy3;
  106.       YB+=BFlysBy3;
  107.       YC+=BFlysBy3;
  108.       YD+=BFlysBy3;
  109.  
  110.     }  /* Group */
  111.  
  112.     Groups<<=2;
  113.     GroupsBy2<<=2;
  114.     GroupsBy3<<=2;
  115.     BFlys>>=2;
  116.     BFlysBy3>>=2;
  117.   }  /* Stage */
  118.  
  119.   /* Unscramble */
  120.   for (i=0;i<N;i++) OutputRe[i]=ReplaceXData[UnscrambledIndex[i]];
  121.   for (i=0;i<N;i++) OutputIm[i]=ReplaceYData[UnscrambledIndex[i]];
  122. }   /* FFT */
  123.  
  124.  
  125.  
  126. void IFFT(double far *InputRe,double far *InputIm,double far *Output)
  127. {
  128.   int BFlys=N_DIV_4,BFlysBy3=(N*3)/4;
  129.   int Groups=1,GroupsBy2=2,GroupsBy3=3;
  130.   int Stage,Group,BFly;
  131.   int i;
  132.  
  133.   double *XA,*XB,*XC,*XD;
  134.   double *YA,*YB,*YC,*YD;
  135.   double xa,xb,xc,xd;
  136.   double ya,yb,yc,yd;
  137.   double *Cb,*Cc,*Cd;
  138.   double *Sb,*Sc,*Sd;
  139.  
  140.   // FFT(complex toegevoegde)=N*IFFT
  141.   for (i=0;i<N;i++) ReplaceXData[i]=InputRe[i];
  142.   for (i=0;i<N;i++) ReplaceYData[i]=-InputIm[i];
  143.  
  144.   for (Stage=LOG2Ndiv2;Stage;Stage--)
  145.   {
  146.     XA=ReplaceXData;
  147.     XB=XA+BFlys;
  148.     XC=XB+BFlys;
  149.     XD=XC+BFlys;
  150.     YA=ReplaceYData;
  151.     YB=YA+BFlys;
  152.     YC=YB+BFlys;
  153.     YD=YC+BFlys;
  154.     for(Group=Groups;Group;Group--)
  155.     {
  156.       Cb=Cc=Cd=SinTwids+N_DIV_4;
  157.       Sb=Sc=Sd=SinTwids;
  158.       for(BFly=BFlys;BFly;BFly--)
  159.       {
  160.         xa=*XA; xb=*XB; xc=*XC; xd=*XD;
  161.         ya=*YA; yb=*YB; yc=*YC; yd=*YD;
  162.  
  163.         *XA++=xa+xb+xc+xd;
  164.         *YA++=ya+yb+yc+yd;
  165.         *XB++=(xa+yb-xc-yd)*(*Cb)+(ya-xb-yc+xd)*(*Sb);
  166.         *YB++=(ya-xb-yc+xd)*(*Cb)-(xa+yb-xc-yd)*(*Sb);
  167.         *XC++=(xa-xb+xc-xd)*(*Cc)+(ya-yb+yc-yd)*(*Sc);
  168.         *YC++=(ya-yb+yc-yd)*(*Cc)-(xa-xb+xc-xd)*(*Sc);
  169.         *XD++=(xa-yb-xc+yd)*(*Cd)+(ya+xb-yc-xd)*(*Sd);
  170.         *YD++=(ya+xb-yc-xd)*(*Cd)-(xa-yb-xc+yd)*(*Sd);
  171.  
  172.         Cb+=Groups;
  173.         Cc+=GroupsBy2;
  174.         Cd+=GroupsBy3;
  175.  
  176.         Sb+=Groups;
  177.         Sc+=GroupsBy2;
  178.         Sd+=GroupsBy3;
  179.       }  /* ButterFly */
  180.  
  181.       XA+=BFlysBy3;
  182.       XB+=BFlysBy3;
  183.       XC+=BFlysBy3;
  184.       XD+=BFlysBy3;
  185.  
  186.       YA+=BFlysBy3;
  187.       YB+=BFlysBy3;
  188.       YC+=BFlysBy3;
  189.       YD+=BFlysBy3;
  190.  
  191.     }  /* Group */
  192.  
  193.     Groups<<=2;
  194.     GroupsBy2<<=2;
  195.     GroupsBy3<<=2;
  196.     BFlys>>=2;
  197.     BFlysBy3>>=2;
  198.   }  /* Stage */
  199.  
  200.   /* Unscramble */
  201.   for (i=0;i<N;i++) Output[i]=ReplaceXData[UnscrambledIndex[i]]/N;
  202. }   /* FFT */
  203.  
  204.